using Devil;
using System.Collections;
using System.Collections.Generic;
using UnityEngine;
using static DynamicBone.DynamicBoneColliderBase;

namespace GameToolkit
{

    public enum ESplitSpaceMode
    {
        Auto,
        _2D_XZ,
        _2D_XY,
        _2D_YZ,
        _3D,
        Auto_2D_XZ,
        Auto_2D_XY,
        Auto_2D_YZ,
    }

    /// <summary>
    /// 访问 BSP 数据
    /// </summary>
    /// <param name="volume"></param>
    /// <returns>如果需要继续访问数据，返回 true， 否则返回 false </returns>
    public delegate bool VisitBspDataAndContinue(IVolumetric volume);

    /// <summary>
    /// 捕获 BSP 空间单元
    /// </summary>
    /// <param name="bounds"></param>
    /// <returns></returns>
    public delegate bool CatchBspCell(Bounds bounds);

    /// <summary>
    /// 二分空间划分，不同于 BSPVolume， BspBounds 将始终划分到最小空间，如果 IVolume 跨越多个空间，将被多个空间保留
    /// </summary>
    public class BspBounds : IEnumerable<IVolumetric>
    {
        public enum EIntersection : byte
        {
            NoIntersect,
            Included,
            Intersect,
        }

        public interface ISpace : IEnumerable<LocalBounds>
        {
            int Version { get; }
            int Deep { get; }
            Bounds localBounds { get; }
            //Matrix4x4 worldToLocalMatrix { get; }
            ISpace LeftChild { get; }
            ISpace RightChild { get; }
            EIntersection GetIntersectType(Bounds bounds);
        }

        #region Btree Node

        public class LocalBounds
        {
            public readonly int id;
            public readonly IVolumetric volume;
            internal BspBounds bsp;
            internal Bounds localBounds;
            internal int version;
            //// 被包含空间数
            //internal int spaceRefers;
            //public int SpaceReferences { get { return spaceRefers; } }

            internal LocalBounds(BspBounds bsp, IVolumetric volume, Bounds bounds)
            {
                id = volume.GetHashCode();
                this.bsp = bsp;
                this.volume = volume;
                localBounds = bounds;
            }

            public void Release()
            {
                var bspinst = bsp;
                bsp = null;
                if (bspinst != null)
                    bspinst.Remove(this);
            }

            public void ResetBounds()
            {
                if (bsp != null)
                {
                    bsp.ResetBounds(this);
                }
            }

            public void SetDirty()
            {
                if (bsp != null)
                {
                    if (!bsp.ResetBounds(this))
                    {
                        bsp.mRoot.SetDirty(this);
                    }
                }
            }
        }

        internal class Leaf
        {
            internal readonly LocalBounds bounds;
            internal Leaf next;
            internal Leaf prev;
            internal Leaf(LocalBounds bounds)
            {
                this.bounds = bounds;
            }
        }

        internal abstract class SpaceNode : ISpace
        {
            internal readonly int deep;
            internal readonly BspBounds bsp;
            internal readonly Bounds _bounds;
            internal int version;
            internal SpaceNode parent;
            internal SpaceNode left;
            internal SpaceNode right;
            internal Leaf first;
            //internal bool split;
            public int Version { get { return version; } }
            public int Deep { get { return deep; } }
            public Bounds localBounds { get { return _bounds; } }
            //public Matrix4x4 worldToLocalMatrix { get { return bsp.mToLocal; } }
            public ISpace LeftChild { get { return left; } }
            public ISpace RightChild { get { return right; } }
            internal SpaceNode(BspBounds bsp, int deep, Bounds bounds)
            {
                this.bsp = bsp;
                this.deep = deep;
                this._bounds = bounds;
            }
            public abstract EIntersection GetIntersectType(Bounds bounds);
            internal abstract SpaceNode CreateChild(Bounds bounds);
            internal void GetSubSpace(out Bounds left, out Bounds right)
            {
                var normal = bsp.mCullNormals[deep % bsp.mCullVector];
                var center = _bounds.center;
                var size = _bounds.size;
                var dir = MathExt.Mul(size, normal) * 0.25f;
                size -= dir * 2f;
                left = new Bounds(center - dir, size);
                right = new Bounds(center + dir, size);
            }
            internal bool IsEmpty
            {
                get
                {
                    return first == null && left == null;
                    //if (left != null)
                    //    return left.IsEmpty && right.IsEmpty;
                    //return true;
                }
            }

            internal void NewVersion()
            {
                var p = this;
                while (p != null)
                {
                    p.version++;
                    p = p.parent;
                }
            }

            void Split()
            {
                if (left == null)
                {
                    Bounds l, r;
                    GetSubSpace(out l, out r);
                    left = CreateChild(l);
                    right = CreateChild(r);
                }
            }

            internal Leaf AddLeaf(LocalBounds bd)
            {
                var leaf = new Leaf(bd);
                if (first == null)
                {
                    first = leaf;
                }
                else
                {
                    leaf.next = first;
                    first.prev = leaf;
                    first = leaf;
                }
                //bd.spaceRefers++;
                return leaf;
            }

            internal bool Add(LocalBounds bd)
            {
                var intersect = GetIntersectType(bd.localBounds);
                if (intersect == EIntersection.NoIntersect)
                {
                    return false;
                }
                else if (intersect == EIntersection.Included || deep == bsp.mDeep)
                {
                    version++;
                    AddLeaf(bd);
                    return true;
                }
                else
                {
                    Split();
                    var add = left.Add(bd);
                    add |= right.Add(bd);
                    if (add) version++;
                    return add;
                }
            }

            void CleanEmptySpaceInParents()
            {
                var p = parent;
                while (p != null/* && !p.split*/)
                {
                    if (p.left == null || (p.left.IsEmpty && p.right.IsEmpty))
                    {
                        p.left = null;
                        p.right = null;
                        p = p.parent;
                    }
                    else
                    {
                        break;
                    }
                }
            }

            internal bool RemoveLeaf(LocalBounds bd)
            {
                var node = first;
                while (node != null)
                {
                    if (node.bounds == bd)
                    {
                        var prev = node.prev;
                        var next = node.next;
                        node.next = null;
                        node.prev = null;
                        if (prev != null)
                            prev.next = next;
                        if (next != null)
                            next.prev = prev;
                        if (first == node)
                            first = next;
                        //bd.spaceRefers--;
                        CleanEmptySpaceInParents();
                        return true;
                    }
                    node = node.next;
                }
                return false;
            }

            internal bool Remove(LocalBounds bd)
            {
                var intersect = GetIntersectType(bd.localBounds);
                if (intersect == EIntersection.NoIntersect)
                {
                    return false;
                }
                if (intersect == EIntersection.Included || deep == bsp.mDeep)
                {
                    var remov = RemoveLeaf(bd);
                    if (remov) version++;
                    return remov;
                }
                if (left != null)
                {
                    var remov = left.Remove(bd);
                    remov |= right != null && right.Remove(bd);
                    if (remov) version++;
                    return remov;
                }
                return false;
            }

            internal void Clear()
            {
                if (left != null || first != null)
                {
                    version++;
                }
                left = null;
                right = null;
                first = null;
                CleanEmptySpaceInParents();
            }

            internal int RemoveLeafs(bool recursive, Bounds bounds, FilterDelegate<IVolumetric> where)
            {
                if (!MathExt.IsIntersects(_bounds, bounds))
                    return 0;
                int num = 0;
                var item = first;
                while (item != null)
                {
                    var bd = item.bounds;
                    if (MathExt.IsIntersects(bounds, bd.localBounds) && (where == null || where(bd.volume)))
                    {
                        var prev = item.prev;
                        var next = item.next;
                        item.prev = null;
                        item.next = null;
                        if (prev != null)
                            prev.next = next;
                        if (next != null)
                            next.prev = prev;
                        if (first == item)
                            first = next;
                        //bd.spaceRefers--;
                        num++;
                        item = next;
                    }
                    else
                    {
                        item = item.next;
                    }
                }
                if (recursive)
                {
                    if (left != null)
                        num += left.RemoveLeafs(recursive, bounds, where);
                    if (right != null)
                        num += right.RemoveLeafs(recursive, bounds, where);
                }
                if (num > 0)
                {
                    version++;
                    CleanEmptySpaceInParents();
                }
                return num;
            }

            internal void ResetBoundsRecursive(LocalBounds bd, Bounds newBounds)
            {
                var intersect = GetIntersectType(newBounds);
                if (intersect == EIntersection.NoIntersect)
                {
                    Remove(bd);
                    return;
                }
                var prev = GetIntersectType(bd.localBounds);
                var insert = intersect == EIntersection.Included || deep == bsp.mDeep;
                var wasInsert = prev == EIntersection.Included || (deep == bsp.mDeep && prev == EIntersection.Intersect);
                version++;
                if (insert)
                {
                    if (!wasInsert)
                    {
                        AddLeaf(bd);
                    }
                    if (left != null)
                    {
                        left.Remove(bd);
                    }
                    if (right != null)
                    {
                        right.Remove(bd);
                    }
                    return;
                }
                if (wasInsert)
                {
                    RemoveLeaf(bd);
                }
                Split();
                //split = true;
                left.ResetBoundsRecursive(bd, newBounds);
                right.ResetBoundsRecursive(bd, newBounds);
                //split = false;
            }

            internal Leaf Get(IVolumetric vol)
            {
                var node = first;
                while (node != null)
                {
                    if (node.bounds.volume == vol)
                    {
                        return node;
                    }
                    node = node.next;
                }
                return null;
            }

            internal bool SetDirty(LocalBounds bd)
            {
                var intersect = GetIntersectType(bd.localBounds);
                if (intersect == EIntersection.NoIntersect)
                    return false;
                if (intersect == EIntersection.Included || deep == bsp.mDeep)
                {
                    version++;
                    return true;
                }
                if (left != null)
                {
                    var dirty = left.SetDirty(bd);
                    dirty |= right.SetDirty(bd);
                    if (dirty) version++;
                    return dirty;
                }
                return false;
            }

            public IEnumerator<LocalBounds> GetEnumerator()
            {
                return new SpaceIterator(this);
            }

            IEnumerator IEnumerable.GetEnumerator()
            {
                return new SpaceIterator(this);
            }
        }

        internal class SpaceOnXoZ : SpaceNode
        {
            internal SpaceOnXoZ(BspBounds bsp, int deep, Bounds bounds) : base(bsp, deep, bounds) { }
            public override EIntersection GetIntersectType(Bounds bounds)
            {
                var min = bounds.min;
                var max = bounds.max;
                var lmin = _bounds.min;
                var lmax = _bounds.max;
                if (lmin.x >= max.x || lmin.y >= max.y || lmin.z >= max.z
                    || lmax.x <= min.x || lmax.y <= min.y || lmax.z <= min.z)
                    return EIntersection.NoIntersect;
                if (min.x <= lmin.x && min.z <= lmin.z && max.x >= lmax.x && max.z >= lmax.z)
                    return EIntersection.Included;
                return EIntersection.Intersect;
            }
            internal override SpaceNode CreateChild(Bounds bounds)
            {
                var child = new SpaceOnXoZ(bsp, deep + 1, bounds);
                child.parent = this;
                return child;
            }
        }

        internal class SpaceOnXoY : SpaceNode
        {
            internal SpaceOnXoY(BspBounds bsp, int deep, Bounds bounds) : base(bsp, deep, bounds) { }
            public override EIntersection GetIntersectType(Bounds bounds)
            {
                var min = bounds.min;
                var max = bounds.max;
                var lmin = _bounds.min;
                var lmax = _bounds.max;
                if (lmin.x >= max.x || lmin.y >= max.y || lmin.z >= max.z
                    || lmax.x <= min.x || lmax.y <= min.y || lmax.z <= min.z)
                    return EIntersection.NoIntersect;
                if (min.x <= lmin.x && min.y <= lmin.y && max.x >= lmax.x && max.y >= lmax.y)
                    return EIntersection.Included;
                return EIntersection.Intersect;
            }
            internal override SpaceNode CreateChild(Bounds bounds)
            {
                var child = new SpaceOnXoY(bsp, deep + 1, bounds);
                child.parent = this;
                return child;
            }
        }

        internal class SpaceOnYoZ : SpaceNode
        {
            internal SpaceOnYoZ(BspBounds bsp, int deep, Bounds bounds) : base(bsp, deep, bounds) { }
            public override EIntersection GetIntersectType(Bounds bounds)
            {
                var min = bounds.min;
                var max = bounds.max;
                var lmin = _bounds.min;
                var lmax = _bounds.max;
                if (lmin.x >= max.x || lmin.y >= max.y || lmin.z >= max.z
                    || lmax.x <= min.x || lmax.y <= min.y || lmax.z <= min.z)
                    return EIntersection.NoIntersect;
                if (min.z <= lmin.z && min.y <= lmin.y && max.z >= lmax.z && max.y >= lmax.y)
                    return EIntersection.Included;
                return EIntersection.Intersect;
            }
            internal override SpaceNode CreateChild(Bounds bounds)
            {
                var child = new SpaceOnYoZ(bsp, deep + 1, bounds);
                child.parent = this;
                return child;
            }
        }

        internal class Space3D : SpaceNode
        {
            internal Space3D(BspBounds bsp, int deep, Bounds bounds) : base(bsp, deep, bounds) { }
            public override EIntersection GetIntersectType(Bounds bounds)
            {
                var min = bounds.min;
                var max = bounds.max;
                var lmin = _bounds.min;
                var lmax = _bounds.max;
                if (lmin.x >= max.x || lmin.y >= max.y || lmin.z >= max.z
                    || lmax.x <= min.x || lmax.y <= min.y || lmax.z <= min.z)
                    return EIntersection.NoIntersect;
                if (min.x <= lmin.x && min.y <= lmin.y && min.z <= lmin.z && max.x >= lmax.x && max.y >= lmax.y && max.z >= lmax.z)
                    return EIntersection.Included;
                return EIntersection.Intersect;
            }
            internal override SpaceNode CreateChild(Bounds bounds)
            {
                var child = new Space3D(bsp, deep + 1, bounds);
                child.parent = this;
                return child;
            }
        }

        #endregion

        #region Bsp Controll

        // 空间迭代器
        struct SpaceIterator : IEnumerator<LocalBounds>
        {
            int _version;
            SpaceNode space;
            Leaf current;
            LocalBounds value;

            internal SpaceIterator(SpaceNode node)
            {
                this.space = node;
                current = node.first;
                _version = node.version;
                value = default;
            }

            public LocalBounds Current { get { return value; } }

            object IEnumerator.Current { get { return value; } }

            public void Dispose()
            {
            }

            public bool MoveNext()
            {
                if (_version != space.version)
                    throw new System.Exception("BspBounds was changed while iterating.");
                if (current != null)
                {
                    value = current.bounds;
                    current = current.next;
                    return true;
                }
                else
                {
                    value = null;
                    return false;
                }
            }

            public void Reset()
            {
                _version = space.version;
                current = space.first;
            }
        }

        public event System.Action<IVolumetric> OnAddVolume;
        public event System.Action<IVolumetric> OnRemoveVolume;
        public event System.Action<IVolumetric> OnRebouding;

        readonly object mLock;
        private readonly int mCullVector;
        private readonly Vector3[] mCullNormals;
        private int mDeep;
        Matrix4x4 mToLocal;
        //Matrix4x4 mToWorld;
        int mRTVersion;
        SpaceNode mRoot;
        int mCount;
        Stack<SpaceNode> mStack;

        bool mGetAllData;
        IVolumetric mCurrentValue;
        FilterDelegate<IVolumetric> mCurrentFilter;
        ICollection<IVolumetric> mCurrentResult;
        VisitBspDataAndContinue mGetVolumeData;


        public int Version { get { return mRoot.version; } }
        internal SpaceNode RootSpace { get { return mRoot; } }
        public ISpace Root { get { return mRoot; } }
        public int Deep { get { return mDeep; } }
        public int Count { get { return mCount; } }
        public Matrix4x4 worldToLocalMatrix { get { return mToLocal; } }
        public Vector3Int Domains
        {
            get
            {
                Vector3Int domains = Vector3Int.zero;
                for (int i = 0; i < mCullNormals.Length; i++)
                {
                    var v = mCullNormals[i];
                    if (Vector3.Dot(v, Vector3.right) > 0.5)
                        domains.x++;
                    else if (Vector3.Dot(v, Vector3.up) > 0.5)
                        domains.y++;
                    else
                        domains.z++;
                }
                return domains;
            }
        }

        public BspBounds(Matrix4x4 worldToLocal, ESplitSpaceMode mode, Bounds bounds, int cullDeep = 12)
        {
            mLock = new object();
            mGetVolumeData = GetCurrentVolume;
            if (mode == ESplitSpaceMode._2D_XZ)
            {
                mCullNormals = new Vector3[] { Vector3.right, Vector3.forward };
                mRoot = new SpaceOnXoZ(this, 0, bounds);
            }
            else if (mode == ESplitSpaceMode._2D_XY)
            {
                mCullNormals = new Vector3[] { Vector3.right, Vector3.up };
                mRoot = new SpaceOnXoY(this, 0, bounds);
            }
            else if (mode == ESplitSpaceMode._2D_YZ)
            {
                mCullNormals = new Vector3[] { Vector3.up, Vector3.forward };
                mRoot = new SpaceOnYoZ(this, 0, bounds);
            }
            else if (mode == ESplitSpaceMode._3D)
            {
                mCullNormals = new Vector3[] { Vector3.right, Vector3.up, Vector3.forward };
                mRoot = new Space3D(this, 0, bounds);
            }
            else
            {
                var size = bounds.extents;
                float weight;
                int[] vecs;
                Vector3[] normals;
                if (mode == ESplitSpaceMode.Auto_2D_YZ)
                {
                    mRoot = new SpaceOnYoZ(this, 0, bounds);
                    vecs = new int[2];
                    weight = size.y + size.z;
                    normals = new Vector3[] { Vector3.up, Vector3.forward };
                    vecs[0] = Mathf.RoundToInt(cullDeep * (size.y / weight));
                    vecs[1] = Mathf.RoundToInt(cullDeep * (size.z / weight));
                }
                else if (mode == ESplitSpaceMode.Auto_2D_XZ)
                {
                    mRoot = new SpaceOnXoZ(this, 0, bounds);
                    vecs = new int[2];
                    weight = size.x + size.z;
                    normals = new Vector3[] { Vector3.right, Vector3.forward };
                    vecs[0] = Mathf.RoundToInt(cullDeep * (size.x / weight));
                    vecs[1] = Mathf.RoundToInt(cullDeep * (size.z / weight));
                }
                else if (mode == ESplitSpaceMode.Auto_2D_XY)
                {
                    mRoot = new SpaceOnXoY(this, 0, bounds);
                    vecs = new int[2];
                    weight = size.x + size.y;
                    normals = new Vector3[] { Vector3.right, Vector3.up };
                    vecs[0] = Mathf.RoundToInt(cullDeep * (size.x / weight));
                    vecs[1] = Mathf.RoundToInt(cullDeep * (size.y / weight));
                }
                else
                {
                    mRoot = new Space3D(this, 0, bounds);
                    vecs = new int[3];
                    weight = size.x + size.y + size.z;
                    normals = new Vector3[] { Vector3.right, Vector3.up, Vector3.forward };
                    vecs[0] = Mathf.RoundToInt(cullDeep * (size.x / weight));
                    vecs[1] = Mathf.RoundToInt(cullDeep * (size.y / weight));
                    vecs[2] = Mathf.RoundToInt(cullDeep * (size.z / weight));
                }
                mCullNormals = new Vector3[cullDeep];
                for (int i = 0; i < cullDeep; i++)
                {
                    int max = 0;
                    float vsize = 0;
                    for (int k = 0; k < normals.Length; k++)
                    {
                        var cullsize = Vector3.Dot(size, normals[k]);
                        if (cullsize > vsize)
                        {
                            vsize = cullsize;
                            max = k;
                        }
                    }
                    var vec = normals[max];
                    mCullNormals[i] = vec;
                    size -= new Vector3(size.x * vec.x * 0.5f, size.y * vec.y * 0.5f, size.z * vec.z * 0.5f);
                }
            }
            mToLocal = worldToLocal;
            mCullVector = mCullNormals.Length;
            mDeep = cullDeep;
        }

        bool GetCurrentVolume(IVolumetric volume)
        {
            if (mCurrentFilter == null || mCurrentFilter(volume))
            {
                if (mGetAllData)
                    mCurrentResult.Add(volume);
                else
                    mCurrentValue = volume;
                return mGetAllData;
            }
            else
            {
                return true;
            }
        }

        public ISpace FindRootSpace(Bounds bounds)
        {
            lock (mLock)
            {
                if (mStack == null)
                    mStack = new Stack<SpaceNode>(mDeep << 1);
                else
                    mStack.Clear();
                SpaceNode space = null;
                if (MathExt.IsIntersects(mRoot._bounds, bounds))
                {
                    mStack.Push(mRoot);
                }
                while (mStack.Count > 0)
                {
                    space = mStack.Pop();
                    if (space.left == null)
                        break;
                    var left = MathExt.IsIntersects(space.left._bounds, bounds);
                    var right = MathExt.IsIntersects(space.right._bounds, bounds);
                    if (left ^ right)
                    {
                        if (left)
                            mStack.Push(space.left);
                        else
                            mStack.Push(space.right);
                    }
                    else
                    {
                        break;
                    }
                }
                return space;
            }
        }

        Leaf FindLocalBounds(IVolumetric volume, Bounds bounds)
        {
            if (mStack == null)
                mStack = new Stack<SpaceNode>(mDeep << 1);
            else
                mStack.Clear();
            mStack.Push(mRoot);
            while (mStack.Count > 0)
            {
                var space = mStack.Pop();
                var intersects = space.GetIntersectType(bounds);
                if (intersects == EIntersection.NoIntersect)
                    continue;
                if (intersects == EIntersection.Included || space.deep == mDeep)
                {
                    var leaf = space.Get(volume);
                    return leaf;
                }
                if (space.left != null)
                {
                    mStack.Push(space.left);
                    mStack.Push(space.right);
                }
            }
            return null;
        }

        bool ResetBounds(LocalBounds bd)
        {
            Bounds newBounds;
            MathExt.ToLocalBounds(mToLocal * bd.volume.localToWorldMatrix, bd.volume.localBounds, out newBounds);
            if (newBounds == bd.localBounds)
                return false;
            int dirtyFlag = 0;
            lock (mLock)
            {
                var remove = mRoot.Remove(bd);
                bd.localBounds = newBounds;
                var add = mRoot.Add(bd);
                if (remove ^ add)
                {
                    dirtyFlag = add ? 1 : 2;
                    if (remove)
                        mCount--;
                    else
                        mCount++;
                }
                else
                {
                    dirtyFlag = 3;
                }
            }
            if (dirtyFlag != 0)
            {
                try
                {
                    if (dirtyFlag == 1)
                        OnAddVolume?.Invoke(bd.volume);
                    else if (dirtyFlag == 2)
                        OnRemoveVolume?.Invoke(bd.volume);
                    else if (dirtyFlag == 3)
                        OnRebouding?.Invoke(bd.volume);
                }
                catch (System.Exception e)
                {
                    Debug.LogException(e);
                }
            }
            return true;
        }

        public void Clear()
        {
            lock (mLock)
            {
                mRoot.Clear();
            }
        }

        public LocalBounds Add(IVolumetric volume)
        {
            LocalBounds bd;
            bool invokeAdd = false;
            lock (mLock)
            {
                Bounds bounds;
                MathExt.ToLocalBounds(mToLocal * volume.localToWorldMatrix, volume.localBounds, out bounds);
                var vol = FindLocalBounds(volume, bounds);
                if (vol != null)
                    return vol.bounds;
                bd = new LocalBounds(this, volume, bounds);
                if (mRoot.Add(bd))
                {
                    bd.version = mRTVersion;
                    mCount++;
                    invokeAdd = true;
                }
            }
            if (invokeAdd)
            {
                try
                {
                    OnAddVolume?.Invoke(volume);
                }
                catch (System.Exception e)
                {
                    Debug.LogException(e);
                }
            }
            return bd;
        }

        public bool Remove(LocalBounds bd)
        {
            bool invokeRemove = false;
            lock (mLock)
            {
                if (mRoot.Remove(bd))
                {
                    invokeRemove = true;
                    mCount--;
                }
            }
            if (invokeRemove)
            {
                try
                {
                    OnRemoveVolume?.Invoke(bd.volume);
                }
                catch (System.Exception e)
                {
                    Debug.LogException(e);
                }
            }
            return invokeRemove;
        }

        // 为了避免不必要的性能开销，调用该方法前，应该确认 volume 已经被添加，否则将造成不必要的查找代缴
        public bool Remove(IVolumetric volume)
        {
            lock (mLock)
            {
                Bounds bounds;
                MathExt.ToLocalBounds(mToLocal * volume.localToWorldMatrix, volume.localBounds, out bounds);
                var leaf = FindLocalBounds(volume, bounds);
                if (leaf == null)
                {
                    leaf = FindLocalBounds(volume, mRoot._bounds);
                }
                if (leaf != null)
                {
                    return Remove(leaf.bounds);
                }
                return false;
            }
        }

        public void RemoveVolumeDatas(Bounds bounds, FilterDelegate<IVolumetric> where)
        {
            lock (mLock)
            {
                mRoot.RemoveLeafs(true, bounds, where);
            }
        }

        public void VisitVolumeDatas(CatchBspCell catchSpace, VisitBspDataAndContinue action)
        {
            if (action == null || catchSpace == null)
                return;
            lock (mLock)
            {
                var ver = ++mRTVersion;
                if (mStack == null)
                    mStack = new Stack<SpaceNode>();
                else
                    mStack.Clear();
                if (catchSpace(mRoot._bounds))
                    mStack.Push(mRoot);
                while (mStack.Count > 0)
                {
                    var space = mStack.Pop();
                    var item = space.first;
                    while (item != null)
                    {
                        var bd = item.bounds;
                        item = item.next;
                        if (bd.version != ver)
                        {
                            bd.version = ver;
                            var keepGoing = action(bd.volume);
                            if (!keepGoing)
                            {
                                mStack.Clear();
                                return;
                            }
                        }
                    }
                    if (space.left != null)
                    {
                        if (catchSpace(space.left._bounds))
                            mStack.Push(space.left);
                        if (catchSpace(space.right._bounds))
                            mStack.Push(space.right);
                    }
                }
            }
        }

        public void VisitVolumeDatas(Vector3 atPosition, VisitBspDataAndContinue action)
        {
            if (action == null)
                return;
            lock (mLock)
            {
                var ver = ++mRTVersion;
                if (mStack == null)
                    mStack = new Stack<SpaceNode>();
                else
                    mStack.Clear();
                if (mRoot._bounds.Contains(atPosition))
                    mStack.Push(mRoot);
                while (mStack.Count > 0)
                {
                    var space = mStack.Pop();
                    var item = space.first;
                    while (item != null)
                    {
                        var bd = item.bounds;
                        item = item.next;
                        if (bd.version != ver && bd.localBounds.Contains(atPosition))
                        {
                            bd.version = ver;
                            var keepGoing = action(bd.volume);
                            if (!keepGoing)
                            {
                                mStack.Clear();
                                return;
                            }
                        }
                    }
                    if (space.left != null)
                    {
                        if (space.left._bounds.Contains(atPosition))
                            mStack.Push(space.left);
                        if (space.right._bounds.Contains(atPosition))
                            mStack.Push(space.right);
                    }
                }
            }
        }

        public void VisitVolumeDatas(Bounds bounds, VisitBspDataAndContinue action)
        {
            if (action == null)
                return;
            lock (mLock)
            {
                var ver = ++mRTVersion;
                if (mStack == null)
                    mStack = new Stack<SpaceNode>(mDeep << 1);
                else
                    mStack.Clear();
                if (MathExt.IsIntersects(mRoot._bounds, bounds))
                    mStack.Push(mRoot);
                while (mStack.Count > 0)
                {
                    var space = mStack.Pop();
                    var item = space.first;
                    while (item != null)
                    {
                        var bd = item.bounds;
                        item = item.next;
                        if (bd.version != ver && MathExt.IsIntersects(bd.localBounds, bounds))
                        {
                            bd.version = ver;
                            var keepGoing = action(bd.volume);
                            if (!keepGoing)
                            {
                                mStack.Clear();
                                return;
                            }
                        }
                    }
                    if (space.left != null)
                    {
                        if (MathExt.IsIntersects(space.left._bounds, bounds))
                            mStack.Push(space.left);
                        if (MathExt.IsIntersects(space.right._bounds, bounds))
                            mStack.Push(space.right);
                    }
                }
            }
        }

        public void VisitVolumeDatas(Matrix4x4 toFrustum, Vector4 frustumClip, VisitBspDataAndContinue action)
        {
            if (action == null)
                return;
            lock (mLock)
            {
                var ver = ++mRTVersion;
                if (mStack == null)
                    mStack = new Stack<SpaceNode>(mDeep << 1);
                else
                    mStack.Clear();
                if (MathExt.IsInFrustum(VolumetricBounds.CalcuateBounds(toFrustum, mRoot._bounds),
                            frustumClip.w, frustumClip.z, frustumClip.x, frustumClip.y))
                {
                    mStack.Push(mRoot);
                }

                Bounds bounds;
                while (mStack.Count > 0)
                {
                    var grp = mStack.Pop();
                    var item = grp.first;
                    while (item != null)
                    {
                        var bd = item.bounds;
                        item = item.next;
                        bounds = VolumetricBounds.CalcuateBounds(toFrustum, bd.localBounds);
                        if (bd.version != ver && MathExt.IsInFrustum(bounds, frustumClip.w, frustumClip.z, frustumClip.x, frustumClip.y))
                        {
                            bd.version = ver;
                            var keepGoing = action(bd.volume);
                            if (!keepGoing)
                            {
                                mStack.Clear();
                                return;
                            }
                        }
                    }
                    if (grp.left != null)
                    {
                        if (MathExt.IsInFrustum(VolumetricBounds.CalcuateBounds(toFrustum, grp.left._bounds),
                            frustumClip.w, frustumClip.z, frustumClip.x, frustumClip.y))
                        {
                            mStack.Push(grp.left);
                        }
                        if (MathExt.IsInFrustum(VolumetricBounds.CalcuateBounds(toFrustum, grp.right._bounds),
                            frustumClip.w, frustumClip.z, frustumClip.x, frustumClip.y))
                        {
                            mStack.Push(grp.right);
                        }
                    }
                }
            }
        }

        public IVolumetric FindVolumeData(Vector3 atPosition, FilterDelegate<IVolumetric> where)
        {
            lock (mLock)
            {
                mGetAllData = false;
                mCurrentFilter = where;
                mCurrentValue = null;
                VisitVolumeDatas(atPosition, mGetVolumeData);
                var ret = mCurrentValue;
                mCurrentValue = null;
                mCurrentFilter = null;
                return ret;
            }
        }

        public void GetVolumeDatas(Vector3 atPosition, ICollection<IVolumetric> result, FilterDelegate<IVolumetric> where)
        {
            if (result == null)
                return;
            lock (mLock)
            {
                mGetAllData = true;
                mCurrentFilter = where;
                mCurrentResult = result;
                VisitVolumeDatas(atPosition, mGetVolumeData);
                mCurrentFilter = null;
                mCurrentResult = null;
            }
        }

        public IVolumetric FindVolumeData(Bounds bounds, FilterDelegate<IVolumetric> where)
        {
            lock (mLock)
            {
                mGetAllData = false;
                mCurrentFilter = where;
                mCurrentValue = null;
                VisitVolumeDatas(bounds, mGetVolumeData);
                var ret = mCurrentValue;
                mCurrentValue = null;
                mCurrentFilter = null;
                return ret;
            }
        }

        public void GetVolumeDatas(Bounds bounds, ICollection<IVolumetric> result, FilterDelegate<IVolumetric> where)
        {
            if (result == null)
                return;
            lock (mLock)
            {
                mGetAllData = true;
                mCurrentFilter = where;
                mCurrentResult = result;
                VisitVolumeDatas(bounds, mGetVolumeData);
                mCurrentFilter = null;
                mCurrentResult = null;
            }
        }

        public IVolumetric FindVolumeData(Matrix4x4 toFrustum, Vector4 frustumClip, FilterDelegate<IVolumetric> where)
        {
            lock (mLock)
            {
                mGetAllData = false;
                mCurrentFilter = where;
                mCurrentValue = null;
                VisitVolumeDatas(toFrustum, frustumClip, mGetVolumeData);
                var ret = mCurrentValue;
                mCurrentValue = null;
                mCurrentFilter = null;
                return ret;
            }
        }

        // 获取视锥范围内的对象
        public void GetVolumeDatas(Matrix4x4 toFrustum, Vector4 frustumClip, ICollection<IVolumetric> result, FilterDelegate<IVolumetric> where)
        {
            if (result == null)
                return;
            lock (mLock)
            {
                mGetAllData = true;
                mCurrentFilter = where;
                mCurrentResult = result;
                VisitVolumeDatas(toFrustum, frustumClip, mGetVolumeData);
                mCurrentFilter = null;
                mCurrentResult = null;
            }
        }

        #endregion

        #region Enumerable

        readonly GetterDelegate<LocalBounds, IVolumetric> mVolGetter = (vol) => vol.volume;

        IEnumerator<IVolumetric> IEnumerable<IVolumetric>.GetEnumerator()
        {
            return new WrapEnumerator<LocalBounds, IVolumetric>(mRoot.GetEnumerator(), mVolGetter);
        }
        IEnumerator IEnumerable.GetEnumerator()
        {
            return new WrapEnumerator<LocalBounds, IVolumetric>(mRoot.GetEnumerator(), mVolGetter);
        }
        #endregion

        #region Gizmos

        void DrawSplitGizmo(SpaceNode grp)
        {
            var size = grp._bounds.extents;
            var normal = mCullNormals[grp.deep % mCullVector];
            var dir = normal; // grp.normal;
            dir.x = 1 - dir.x;
            dir.y = 1 - dir.y;
            dir.z = 1 - dir.z;
            var dir2 = Vector3.Cross(dir, normal);
            dir2 = MathExt.Mul(dir2, size);
            var dir3 = Vector3.Cross(normal, dir);
            dir3 = MathExt.Mul(dir3, size);
            dir = MathExt.Mul(dir, size);
            var center = grp._bounds.center;
            Gizmos.DrawLine(center - dir, center + dir2);
            Gizmos.DrawLine(center - dir, center + dir3);
            Gizmos.DrawLine(center + dir, center + dir2);
            Gizmos.DrawLine(center + dir, center + dir3);
        }

        void DrawVolumeBoundsGizmo(IVolumetric bd)
        {
            Gizmos.matrix = bd.localToWorldMatrix;
            Gizmos.DrawWireCube(bd.localBounds.center, bd.localBounds.size);
        }

        public void DrawVolumeGizmo(Bounds volume, Color spaceColor, Color volumeColor)
        {
            lock (mLock)
            {
                var ver = ++mRTVersion;
                var matrix = Matrix4x4.identity;// mToLocal.inverse;
                Matrix4x4.Inverse3DAffine(mToLocal, ref matrix);
                Gizmos.matrix = matrix;
                Gizmos.color = spaceColor;
                Gizmos.DrawWireCube(mRoot._bounds.center, mRoot._bounds.size);
                if (mStack == null)
                    mStack = new Stack<SpaceNode>(mDeep << 1);
                else
                    mStack.Clear();
                if (MathExt.IsIntersects(mRoot._bounds, volume))
                    mStack.Push(mRoot);
                while (mStack.Count > 0)
                {
                    var grp = mStack.Pop();
                    Gizmos.color = volumeColor;
                    var data = grp.first;
                    while (data != null)
                    {
                        if (data.bounds.version != ver && MathExt.IsIntersects(data.bounds.localBounds, volume))
                        {
                            data.bounds.version = ver;
                            DrawVolumeBoundsGizmo(data.bounds.volume);
                            //Gizmos.DrawWireCube(data.bounds.localBounds.center, data.bounds.localBounds.size);
                        }
                        data = data.next;
                    }
                    //if (grp.deep < mDeep)
                    //{
                    //    Gizmos.color = spaceColor;
                    //    DrawSplitGizmo(grp);
                    //}
                    if (grp.left != null)
                    {
                        Gizmos.matrix = matrix;
                        Gizmos.color = spaceColor;
                        DrawSplitGizmo(grp);
                        if (MathExt.IsIntersects(grp.left._bounds, volume))
                            mStack.Push(grp.left);
                        if (MathExt.IsIntersects(grp.right._bounds, volume))
                            mStack.Push(grp.right);
                    }
                }
            }
        }

        public void DrawFrustumGizmo(Matrix4x4 toFrustum, Vector4 frustumClip, Color spaceColor, Color volumeColor)
        {
            lock (mLock)
            {
                if (mStack == null)
                    mStack = new Stack<SpaceNode>(mDeep << 1);
                else
                    mStack.Clear();
                mStack.Push(mRoot);
                var ver = ++mRTVersion;

                Bounds bounds;
                Gizmos.color = spaceColor;
                Gizmos.DrawWireCube(mRoot._bounds.center, mRoot._bounds.size);
                var mat = mToLocal.inverse;// Gizmos.matrix;
                Gizmos.matrix = mat * toFrustum.inverse;
                GizmosUtil.DrawFrustum(frustumClip);
                Gizmos.matrix = mat;
                while (mStack.Count > 0)
                {
                    var grp = mStack.Pop();
                    Gizmos.color = volumeColor;
                    var data = grp.first;
                    while (data != null)
                    {
                        bounds = VolumetricBounds.CalcuateBounds(toFrustum, data.bounds.localBounds);
                        if (data.bounds.version != ver && MathExt.IsInFrustum(bounds, frustumClip.w, frustumClip.z, frustumClip.x, frustumClip.y))
                        {
                            data.bounds.version = ver;
                            DrawVolumeBoundsGizmo(data.bounds.volume);
                            //DrawVolumeBoundsGizmo(data.bounds.volume);
                            //Gizmos.DrawWireCube(data.bounds.localBounds.center, data.bounds.localBounds.size);
                        }
                        data = data.next;
                    }
                    //if (grp.deep < mDeep)
                    //{
                    //    Gizmos.color = spaceColor;
                    //    DrawSplitGizmo(grp);
                    //}
                    if (grp.left != null)
                    {
                        Gizmos.matrix = mat;
                        Gizmos.color = spaceColor;
                        DrawSplitGizmo(grp);

                        if (MathExt.IsInFrustum(VolumetricBounds.CalcuateBounds(toFrustum, grp.left._bounds),
                            frustumClip.w, frustumClip.z, frustumClip.x, frustumClip.y))
                        {
                            mStack.Push(grp.left);
                        }
                        if (MathExt.IsInFrustum(VolumetricBounds.CalcuateBounds(toFrustum, grp.right._bounds),
                            frustumClip.w, frustumClip.z, frustumClip.x, frustumClip.y))
                        {
                            mStack.Push(grp.right);
                        }
                    }
                }
            }
        }
        #endregion
    }

}